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Abstract —In the recent past, several sampling-based al¬ 
gorithms have been proposed to compute trajectories that 
are collision-free and dynamically-feasible. However, the out¬ 
puts of such algorithms are notoriously jagged. In this 
paper, by focusing on robots with car-like dynamics, we 
present a fast and simple heuristic algorithm, named 
Convex Elastic Smoothing (CES) algorithm, for trajec¬ 
tory smoothing and speed optimization. The CES algorithm is 
inspired by earlier work on elastic band planning and iteratively 
performs shape and speed optimization. The key feature of the 
algorithm is that both optimization problems can be solved 
via convex programming, making CES particularly fast. A 
range of numerical experiments show that the CES algorithm 
returns high-quality solutions in a matter of a few hundreds 
of milliseconds and hence appears amenable to a real-time 
implementation. 

1. Introduction 

The problem of planning a collision-free and dynamically- 
feasible trajectory is fundamental in robotics, with appli¬ 
cation to systems as diverse as ground, aerial, and space 
vehicles, surgical robots, and robotic manipulators [1]. A 
common strategy is to decompose the problem in steps of 
computing a collision-free, but possibly highly-suboptimal or 
not even dynamically-feasible trajectory, smoothing it, and 
finally reparameterizing the trajectory so that the robot can 
execute it [2]. In other words, the first step provides a strategy 
that explores the configuration space efficiently and decides 
“where to go,” while the subsequent steps provide a refined 
solution that specifies “how to go ” 

The first step is often accomplished by running a sampling- 
based motion planning algorithm [1], such as PRM [3] 
or RRT [4]. While these algorithms are very effective 
for quickly finding collision-free trajectories in obstacle- 
cluttered environments, they often return jerky, unnatural 
paths [5]. Furthermore, sampling-based algorithms can only 
handle rather simplified dynamic models, due to the com¬ 
plexity of exploring the state space while retaining dynamic 
feasibility of the trajectories. The end result is that the trajec¬ 
tory returned by sampling-based algorithms are characterized 
by jaggedness and are often dynamically-infeasible, which 
requires the subsequent use of algorithms for trajectory 
smoothing and reparametrization. 

Accordingly, the objective of this paper is to design a fast 
and simple heuristic algorithm for trajectory smoothing and 
reparametrization that is amenable to a real-time implemen¬ 
tation, with a focus on mobile robots, in particular robotic 
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cars. Specifically, we seek an algorithm that within a few 
hundreds of milliseconds can turn a jerky trajectory returned 
by a sampling-based motion planner into a smooth, speed- 
optimized trajectory that fulfills strict dynamical constraints 
such as friction, bounded acceleration, or turning radius 
limitations. 

A. Literature Review 

The problem of smoothing a trajectory returned by a 
sampling-based planner is not new and has been studied 
since the introduction of sampling-based algorithms [6]. For 
planning problems that do not involve the fulfillment of 
dynamic constraints (e.g., limited turning radius), efficient 
smoothing algorithms are already available. In this case, the 
most widely applied method is the Shortcut heuristic, 
because of its effectiveness and simple implementation [7]. 
In a typical implementation, this algorithm considers two 
random configurations along the trajectory. If these two con¬ 
figurations can be connected with a new shorter trajectory (as 
computed via a local planner), then the original connection 
is replaced with the new one. 

For planning problems with dynamic constraints, however, 
the situation is more contentious. While several works have 
studied sampling-based algorithms for kinodynamic planning 
[4], [8], [9], [10], [11], [12], relatively few works have 
addressed the issues of trajectory smoothing with dynamic 
constraints. Broadly speaking, current techniques can be 
classified into two categories [5], namely shortcut methods 
and optimization-based methods. Shortcut methods strive to 
emulate the Shortcut algorithm in a kinodynamic context. 
Specifically, jerky portions of a path are replaced with curve 
segments such as parabolic arcs [13], clothoids [14], Bezier 
curves [15], Catmull-Rom splines [16], cubic B-splines [5], 
or Dubins curves [17]. Such methods are rather fast (they 
usually complete in a few seconds), but handle dynamic 
constraints only implicitly, for example, by constraining the 
curvature of the trajectories and/or ensuring continuity. 
Also, they usually do not involve speed optimization along 
the computed trajectory. Notably, two of the main teams in 
the DARPA Grand Challenge, namely team Stanford [18] and 
team CMU [19], applied shortcut methods as their smoothing 
procedure. 

In contrast, optimization-based methods handle dynamic 
constraints explicitly, as needed, for example, for high- 
performance mobile vehicles. Two common approaches are 
gradient-based methods [20] and elastic bands or elastic 
strip planning [21], [22], [23], which model a trajectory as 
an elastic band. These works, however, are mostly geared 
toward robotic manipulators [22], may still require several 
seconds to find a solution [20], and generally do not address 
speed optimization along the trajectory. 



B. Statement of Contributions 


11. Problem Statement 


In this paper, leveraging recent strides in the field of 
convex optimization [24], we design a novel algorithm, 
named Convex Elastic Smoothing (CES) algorithm, 
for trajectory smoothing and speed optimization. The focus 
is on mobile robots with car-like dynamics. Our algorithm 
is inspired by the elastic band approach [23], in that we 
identify a collision-free “tube” around the trajectory returned 
by a sampling-based planner, within which the trajectory is 
“stretched” and speed is optimized. The stretching and speed 
optimization steps rely on convex optimization. In particular, 
the stretching step draws inspiration from [25], while the 
speed optimization step is essentially an implementation of 
the algorithm in [26]. In contrast with [23], our algorithm 
uses convex optimization for the stretching process, performs 
speed optimization, and handles a variety of constraints (e.g., 
friction) that were not considered in [23]. As compared to 
[25], our algorithm handles more general workspaces and 
removes the assumption of a constant speed. 

Specifically, the CES algorithm divides the trajectory 
smoothing process into two steps: (1) given a fixed velocity 
profile along a reference trajectory, optimize the shape of 
the trajectory, and (2) given a fixed shape for the trajectory, 
optimize the speed profile along the trajectory. We show that 
each of the two steps can be readily solved as a convex 
optimization problem. The two steps are then repeated until 
a termination criterion is met (e.g., timeout). In this paper, 
the initial reference trajectory is computed by running the 
differential FMT* algorithm [27], a kinodynamic variant 
of the FMT* algorithm [28]. Numerical experiments on a 
variety of scenarios show that in a few hundreds of millisec¬ 
onds the CES algorithm outputs a “high-quality” trajectory, 
where the jaggedness of the original trajectory is eliminated 
and speed is optimized. Coupled with differential FMT*, 
the CES algorithm is able to find high-quality solutions to 
rather complicated planning problems in well under a second, 
which appears promising for a real-time implementation. 

We mention that the reference trajectory used as an input 
to the CES algorithm can be the output of any sampling- 
based motion planner, and indeed of any motion planning 
algorithm. In particular, the CES algorithm appears to per¬ 
form well even when the reference trajectory is not collision- 
free or does not fulfill some of the dynamic constraints (see 
Section for more details). Also, while in this paper we 
mostly focus on vehicles with second-order, car-like dynam¬ 
ics, the CES algorithm can be generalized to a variety of 
other mobile systems such as aerial vehicles and spacecraft. 


C. Organization 

This paper is structured as follows. In Section 
formally state the problem we wish to solve. In Section III 


we 

we 


present the CES algorithm, a novel algorithm for trajectory 
smoothing that relies on convex optimization and runs in 
a few hundreds of milliseconds. In Section |lv| we present 
results from numerical experiments highlighting the speed 
of the the CES algorithm and the quality of the returned 
solutions. Finally, in Section |Vj we draw some conclusions 
and discuss directions for future work. 


Let W C denote the two-dimensional work space for a 
car-like vehicle. Let O = {Oi, O25 • • •, O^}, with Oi C W, 
i = denote the set of obstacles. For simplicity, 

we assume that the obstacles have polygonal shape. In this 
paper we primarily focus on a unicycle dynamic model 
for the vehicle. Extensions to more sophisticated car-like 
models are discussed in Section Specifically, following 
[26], let q G W represent the position of the vehicle, 
and q and q its velocity and acceleration, respectively. We 
consider a non-drifting and non-reversible car model so that 
the heading of the vehicle is the same as the direction 
of the instantaneous velocity vector, and we denote by 
0(q) the mapping from vehicle’s speed to its heading. The 
control input u = ^ is two-dimensional, with the 

first component, representing longitudinal force and 

the second component, representing lateral force. The 
dynamics of the vehicle are given by 

'cos(/)(q) -sin(/)(q)l 

■ j,r-\ 

sm0(q) cos0(q) 

where m is the vehicle’s mass, see Figure We consider a 
friction circle constraint for u, namely 

||u|| (2) 

where /i is the friction coefficient and g is the gravitational 
acceleration (in this paper, norms should be interpreted as 
2-norms). The longitudinal force is assumed to be upper 
bounded as 

^long ^ ^long^ ^3^ 

where ^ M>o encodes the force limit from the wheel 

drive. Finally, we assume a minimum turning radius Rmin 
for the vehicle, which, in turn, induces a constraint on the 
lateral force according to 

(4) 

-tTmin 

The minimum turning radius (Rmin) depends on specific 
vehicle parameters such as wheelbase and maximum steering 
angle for steering wheels. 

Some comments are in order. First, the unicycle model 
assumes that the heading of the vehicle is the same as the 
direction of the instantaneous velocity vector. This is a rea¬ 
sonable assumption in most practical situations, but becomes 
a poor approximation at high speeds when significant under¬ 
steering takes place, or at extremely low speeds when the 
motion is determined by Ackermann steering geometry. We 
will show, however, that the results presented in this paper 
can be extended to more complex car models, e.g., half-car 
models, by leveraging differential flatness of the dynamics. 
Second, we assume that the actual control inputs such as 
steering and throttle opening angles can be mapped to u 
via a lower-level control algorithm. This is indeed true for 
most ground vehicles, see, e.g., [29]. Third, the friction circle 
model captures the dependency between lateral and longitu¬ 
dinal forces in order to prevent sliding. Fourth, constraint 0 
might yield unbounded speeds. This issue could be addressed 
by conservatively choosing a smaller value of that 









guarantees an upper bound of the achievable speed within 
the planning distance. An alternative formulation is to set a 
limit on the total traction power ^long 11 ^11 ^ where 
W denotes the maximum power provided by the engine. 
This form is closer to the real constraint on traction force, 
but it makes the constraint non-convex - this is a topic left 
for future research. Finally, the model ([T])-0, with minor 
modifications, can be applied to a variety of other vehicles 
and robotic systems, e.g., spacecraft, robotic manipulators, 
and aerial vehicles [26]. Hence, the algorithm presented in 
this paper may be applied to a rather large class of systems 
- this is a topic left for future research. 

We are now in a position to state the problem we wish 
to solve in this paper. Consider a collision-free reference 
trajectory computed, for example, by running a sampling- 
based motion planner [1]. Let this trajectory be discretized 
into a set of waypoints V := {Pq^Pi^ ... ^Pn}, where, 
by construction. Pi G W \ O for i = l,...,n. The 
goal is to design a heuristic smoothing algorithm that uses 
the information about the vehicle’s model (0-0, obstacle 
set O, and (discretized) reference trajectory V to com¬ 
pute a dynamically-feasible (with respect to model ([TJ-Q), 
collision-free, and smooth trajectory that goes from Pq to 
Pn and has an optimized speed profile, see Figure Our 
proposed algorithm is named CES and is presented in the 
next section. 



Fig. 1: The goal of this paper is to design a fast algorithm to 
locally optimize the output of a motion planner, with a focus 
on car models. Specifically, the smoothing algorithm takes 
as input a reference trajectory V and returns a smoothed 
trajectory Q with an optimized speed profile. 


III. The CES Algorithm 


At a high level, the CES algorithm performs the following 
operations. First, a sequence of “bubbles” is placed along 
the reference trajectory in order to identify a region of the 
workspace that is collision free. Such a region can be thought 
of as a collision-free “tube” within which the reference 
trajectory, imagined as an elastic band, can be stretched so 
as to obtain a smoother trajectory. Assuming that a speed 
profile along the reference trajectory is given, such stretching 
procedure can be cast as a convex optimization problem, as 


it will be shown in Section III-B Furthermore, optimizing 
the speed profile along a stretched trajectory can also be 
cast as a convex optimization problem. However, jointly 
stretching a trajectory and optimizing the speed profile is 


Algorithm 1 Bubble generation 

Require: Reference trajectory V, obstacle set O, bubble 
bounds ri and Vu 

1 

for i = 2 : {n — 1) do 

2 

if \\Pi - A^_i|| < 0.5 • Vi^i then 

3 

Bi ^ Bi-i 

4 

Ai ^ Ai_i 

5 


6 

continue 

7 

end if 

8 

Bi ^ GenerateBubble(P^) 

9 

Vi ^ Radius of Bi 

10 

^ Pi 

11 

if Vi < ri then 

12 

Bi ^ TranslateBubble(Ai,r^) 

13 

Vi ^ Radius of Bi 

14 

Ai ^ Center of Bi 

15 

end if 

16 

end for 


a non-convex problem. Hence, the CES algorithm proceeds 
by alternating trajectory stretching and speed optimization. 
Simulation results, presented in Section show that such 
a procedure is amenable to a real-time implementation and 
yields suboptimal, yet high-quality trajectories. Our CES 
algorithm is inspired by the elastic band and bubble method 
[23]. The work in [23], however, mostly focuses on geomet¬ 
ric (i.e., without differential constraints) planning, and does 
not consider speed optimization, as opposed to our problem 
setup. 

In the remainder of this section we present the different 
steps of the CES algorithm, namely, bubble generation, 
elastic stretching and speed optimization. Finally the overall 
CES algorithm is presented. 


A. Bubble Generation 

The first step is to compute a sequence of bubbles, one 
for each waypoint Pi, so as to identify a collision-free tube 
along the reference trajectory for subsequent optimization. 
Since the problem is two-dimensional, each bubble is indeed 
a circle. As discussed, extensions to systems in higher 
dimensions (e.g., airplanes or quadrotors) are possible, but 
are left for future research. The bubble generation algorithm 
is shown in Algorithm 

Let Bi denote the bubble associated with waypoint Pi, 
i = 1,... ,n, and Ai and denote, respectively, its center 
and radius. According to this notation, Bi = {x eyV\\\x — 
^iW < For each waypoint Pi, Algorithmattempts to 
compute a bubble such that: (1) its radius is upper bounded 
by Vu C M >05 (2) whenever possible, its radius is no less 
than ri G ^> 0 , and (3) its center is as close as possible to 
Pi. The role of the upper bound is to limit the smoothing 
procedure within a relatively small portion of the workspace, 
say 10% (in other words, to make the optimization “local”). 
In turn, the minimum bubble radius ri is set according to 
the maximum distance between adjacent waypoints, so that 
every bubble overlaps with its neighboring bubbles and the 
placement of new waypoints for trajectory stretching (see 
Section [ni-B| ) does not have any “gaps”. 
















First, in lines 0 - 0 , the algorithm checks whether Pi 
is “too close” to the center of bubble Bi-i. If this is the 
case, bubble Bi is made equal to bubble Bi-i. Otherwise, 
the algorithm considers as candidate center for bubble Bi 
the waypoint Pi (that is collision-free) and computes, via 
function Gene rat eBubble(P^), the largest bubble cen¬ 
tered at Pi that is collision-free (with maximum radius Vu), 
see lines 0 -([Tg. For rectangular-shaped obstacles, this is 
a straightforward geometrical procedure. If the obstacles 
have more general polygonal shapes, a bisection search is 
performed with respect to the bubble radius. Should the 
resulting radius be lower than the threshold r^, an attempt 
is made to translate Ai so that a larger (collision-free) 
bubble can be placed, lines ([n)-([T5](. Specifically, func¬ 
tion TranslateBubble(74^,r^) first identifies the edge 
of the obstacle closest to Ai (recall that the obstacles are 
assumed of polygonal shape). Then, the outward normal 
direction to the edge is computed and the center of the 
bubble is moved along such direction until a ball of ra¬ 
dius ri can be placed. Should this not be possible, then 
TranslateBubble(74^,r^) returns the ball of largest ra¬ 
dius among the balls whose centers lie on the aforementioned 
normal direction. 

Figure shows an example application of the bubble gen¬ 
eration algorithm. The centers Ai, A 2 and A 4 coincide with 
their corresponding waypoints, while center As is translated 
away from waypoint P 3 to allow for a larger bubble radius. 
Figure shows a typical output of Algorithm 

By construction, the bubble regions are collision-free 
and represent the feasible space for the placement of new 
optimized waypoints during the elastic stretching process. 
Note that in some cases trajectories connecting points in 
adjacent bubbles may be in collision with obstacles, as shown 
in FigureThis issue is mitigated in practice by considering 
a “large enough” number of reference waypoints (possibly 
adding them iteratively) and/or infiating the obstacles. A 
principled way to select the number of waypoints would 
rely on a reachability analysis for the unicycle model 0 - 
0 . However, to minimize computation time, we rely on a 
heuristic choice for the waypoint number. Specifically, the 
spacing between the waypoints is roughly equal to a quarter 
of the car length. 



Fig. 2: Example application of the bubble generation proce¬ 
dure, Algorithmic 

B. Elastic Stretching (aka Shape Optimization) 

The key insight of the elastic stretching procedure is 
to view a trajectory as an elastic band, with n nodal 
points whose positions can be adjusted within the respective 
(collision-free) bubbles. Such nodal points represent the new 
waypoints for the smoothed trajectory, which replace the 
original waypoints in P. Within this perspective, the dynamic 



Fig. 3: Typical output of Algorithm for an environment 
with rectangular-shaped obstacles. 



Fig. 4: Artificial tensile forces and balance force for a 
sequence of points placed in bubbles k — 1 , k, k 1 . 


constraints on the shape of a trajectory are mimicked by the 
bending stiffness of the band. 

Specifically, consider Figure Let Qk and Qk-\-i be 
points, respectively, in bubbles k and k 1 , with k = 
— 1. We consider an artificial tensile force 
between Qk and Qk-\-i given by 

F/c •— Qk-\-l Qk‘ 

Accordingly, the balancing force at point Qk, k = 1,..., n is 


Nfe := Fk-i - Fk. (5) 

As in [23], the physical interpretation is a series of 
springs between the bubbles. From a geometric standpoint, 
the balancing force Nk captures curvature information along 
a trajectory. Note that if all balance forces are equal to zero, 
the trajectory is a straight line, which, clearly, has “ideal 
smoothness.” To smooth a trajectory, the goal is then to 
place new waypoints Qi,..., Qn within the bubbles so as to 
minimize the sum of the norms of the balance forces subject 
to constraints due to the vehicle’s dynamics. In this way, 
the trajectory is “bent” as little as possible in order to avoid 
collisions with obstacles. 

The constraints for the placement of the new waypoints 
rely on a number of approximations to ensure convexity of 
the optimization problem. Specifically, assume the longitudi¬ 
nal force Uk^^ and velocity Vk are given at each waypoint in 
P (their optimization will be discussed in the next section). 
We define Rk as the instantaneous turning radius for the 
car at the kth waypoint. The lateral acceleration for 
waypoints Qk, /c = 2 ,...,n — 1 can be upper bounded as 


= 


l|Vk| 

Rk 


< 




ii^sf - 


long '' 
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'= OLk, 


( 6 ) 
























































where the inequality follows from the friction circle con¬ 
straint Hence, l/Rk < <^/c/||vk|p. To relate Rk with 
||N/c||, we make use of the following approximations, valid 
when the waypoints are uniformly spread over a trajectory 
and dense “enough”: (1) \\Fk\\ = ||F/c_i||, (2) Ok is small, 
and (3) Ok = \\Qk-\-i — Qk\\/Rk- Then one can write 

||N,|| = ||F,_i-F,||^2.||F,||.sin(^,/2) 

— IIQ/e+l - Qk\\ Ok — IIQ/e+1 - Qk\\‘^/Rk (7) 

< IIQ/c+1 - Q/clP Qf/c/livkip. 

Lastly, to make the above inequality a quadratic constraint 
in the QkS variables, we approximate the length of each 
band Qk-\-i — Qk as the average length d along the reference 
trajectory, i.e., 

^._ TVAPk+i-Pk\\ _ 

n — 1 

Note that the minimization of ||N/c|| as the optimization ob¬ 
jective inherently reduces the non-uniformity of the lengths 
of each band, which justifies the above approximation. 

In summary, we obtain the friction constraint 

IlNfell • (8) 

Also, again leveraging equation 10, we obtain the turning 
radius constraint 

IlNfcll < (9) 

-^min 


at the intersection of or a straight line if the 

two velocity vectors are parallel. The speed profile along 
this continuous trajectory may be taken as piecewise linear, 
and the discrete constraints and 0 ensure that the 
continuous constraints 0-0 are satisfied. The quality of 
such trajectories will be investigated in Section 

C. Speed Optimization 

The speed optimization over a fixed trajectory relies on 
the convex optimization algorithm presented in [26]. The 
inputs to this algorithm are (1) a sequence of waypoints 
{Qi,... ,Qn} (representing the trajectory to be followed), 
(2) the friction coefficient ji for the friction circle constraint 
in equation ([^, and (3) the maximum traction force 
defined in equation The outputs are (1) the sequence 
of velocity vectors {vi,...,v^} and (2) the sequence of 
longitudinal control forces one for each 

waypoint /c = 1,..., n. We refer the reader to [26] for 
details about the algorithm. 


D. Overall Algorithm 

The CES algorithm alternates between elastic stretching 
(Section [Ill-B| ) and speed optimization (Section [IlI-C| ), until a 
given tolerance on length reduction, traversal time reduction, 
or a timeout condition are met. Note that at iteration i>2, 
the elastic stretching algorithm should use as estimate for the 
average band length the quantity 


dW := 


E n —1 

k=l 




n — 1 


To constrain the initial and final endpoints of the trajectory, 
one simply imposes Qi = Pi and Qn = Pn- In turn, to 
constrain the initial and final heading angles, one can use the 
constraints Q 2 = Pi+d-^^ and Q„_i = Pn-d- 
Noting thatNfe = 2Qi~ — Qk-i — Qk+i, the elastic stretching 
optimization problem is: 

n—1 

min Y] ||2Qfe - Qfe-i - Qfc+i||^ 

Q3,---,Qn-2 7 -- 

rC — ^ 

s.t. Qi = Pi, Q 2 = Pi + d ■ Tj—^ 

llvill 

Qn = Pn, Qn-1 = Pn - d ■ 2 *^"^ 

l|Vn-l|| 

Q k ^ Pk 5 ^ 3,. .., 77 / 1 

||2^/c Qk—i ^/c-i-i|| ^ min i — , cxk f ^ 

I -^min \Y’/c / 

for /c = 2 ,..., 77/ — 1. 

This is a convex optimization problem with quadratic objec¬ 
tive and quadratic constraints (QCQP), where the decision 
variables are the intermediate waypoints Qk, k = 3,... ,n — 
2, which can be placed anywhere within the collision-free 
regions {Bz,.. .,Bn- 2 }, 

Note that any feasible solution to the above QCQP, output 
as a sequence of discrete waypoints, represents a continuous¬ 
time trajectory satisfying the unicycle model Q-Q. This 
full trajectory may be recovered by interpolating between 
adjacent waypoints Qk,Qk-\-i using circular arcs centered 


where the ^^’s are the waypoints computed at iteration 
i—1. According to our discussion in Section [ni-B[ at iteration 
i = 1 one should set = Pk, for /c = 1,..., tT/. 

IV. Numerical Experiments 

In this section we investigate the effectiveness of the CES 
algorithm along two main dimensions: (1) quality of the 
smoothed trajectory, measured in terms of traversal time 
reduction with respect to the reference trajectory, and (2) 
computation time. We consider three sets of experiments. In 
the first set, we consider 24 random mazes with rectangular¬ 
shaped obstacles, similar to the example in Figure The 
reference trajectory is computed by running the differential 
FMT* algorithm [27]. In the second set, to test the robustness 
of the algorithm, we consider a scenario where the reference 
trajectory is computed disregarding the vehicle’s dynamics. 
This could be the case when, to minimize computation time 
as much as possible, the use of a motion planner is avoided. 
Finally, we consider a scenario where a robotic car is mod¬ 
eled according to a more sophisticated bicycle (equivalently, 
half-car) model. The reference trajectory is computed by 
running differential FMT* on the unicycle model 0-0. 
By leveraging the differential fiatness of the bicycle model, 
the CES algorithm is then applied to the trajectory returned 
by differential FMT* (computed on a different model). This 
scenario represents the typical case whereby one seeks to run 
a motion planner on a simpler model of a vehicle, and then a 
smoothing algorithm on a more refined model. Furthermore, 
this scenario shows how to apply the CES algorithms to 






















vehicle models more general than ©-•EJi- For all scenarios, 
the algorithm is stopped whenever the traversal time at the 
current iteration is no longer reduced with respect to the 
previous iteration. For the bubble generation method, we 
chose Vu = 10 m and ri = 1 m, consistent with the 
workspace dimensions discussed below. 

All numerical experiments were performed on a computer 
with an Intel(R) Core(TM) i7-3632QM, 2.20GHz processor 
and 12GB RAM. The CES algorithm was implemented in 
Matlab with an interface to FORCES Pro 130] for elastic 
stretching and MTSOS 126] for speed optimization. 

A. Random Mazes 

In this scenario the workspace is a 100m x 100m square 
with rectangular-shaped obstacles randomly placed within 
(the obstacle coverage was roughly 50%). The parameters 
for the model in equations Q-Q are m = 833 kg, ji = 
0.8, and [7'°“^ = o.5 jimg. The reference trajectories, 
computed via differential FMT* by using 1,000 samples, 
were discretized into 257 waypoints with an average segment 
length equal to 0.56 m. On average, each iteration (con¬ 
sisting of bubble generation, shape optimization, and speed 
optimization) required 119 ms, with a standard deviation of 
14 ms. Specifically, the bubble generation algorithm required, 
on average, 26ms. The shape optimization algorithm required 
74 ms. Finally, the speed optimization required 19 ms. A 
typical smoothed trajectory is portrayed in Figure The 
traversal time reduction, which is computed according to 
the formula *100%, ranges from a minimum of 

0.2% to a maximum of 18%, with the average value being 
3.54%. Figure shows the smoothed trajectory for one of 
the 24 random mazes. We note that, apart from the benefit 
of reduction of traversal time, a smoothed trajectory may be 
easier to track for a lower-level controller. 



Fig. 5: A typical smoothed trajectory for the random maze 
scenario. In this case, the traversal time reduction is 9.12%. 


B. Lane Changing 

For this scenario, we consider a road lane 50 m long with 
rectangular-shaped obstacles in it. The parameters for the 
model in equations ([T])-(0 are m = 1,725 kg, g = 0.5, 
and = 0.3 gmg. The reference trajectory is generated 

by simply computing the center line of the collision-free 
“tube” along the road. This corresponds to the case where, 
to minimize computation time as much as possible, a refer¬ 
ence trajectory is computed disregarding vehicle’s dynamics. 
Figure shows the smoothed trajectory and speed profile. 


The computation time was 100 ms. This scenario illustrates 
that algorithm CES can also smooth reference trajectories 
that are not dynamically-feasible. Of course, in this case the 
traversal time for the smoothed trajectory is longer, due to 
the dynamic constraints. 


- - Reference path 

— Obstacle edges 
— Smooth result 
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Vmax 



Fig. 6: Smoothed trajectory for the lane changing scenario. 


C. Smoothing with Bicycle Model 

In this scenario, we assume a more sophisticated model 
for the vehicle, namely a half-car (or bicycle) model, see 
Figure [7] This model is widely used when local vehicle states 
such as sideslip angle and yaw rate are of primary interest 
129]. In Figure Pc^ denotes the center of gravity (CG) 
of the car, ^ denotes vehicle’s orientation, and and Vy 
denote components of speed v in a body-fixed axis system. 
Also, If and Ir denote the distances from the CG to the 
front and rear wheels, respectively. Finally, with a G 
{f,r} , G {x^y} denotes the frictional forces of front and 
rear wheels. The control input is represented by the triple 
( = [S, Sfx, Srx]^ where 6 denotes the steering angle of the 
front wheel, and Sfx and Srx denote the longitudinal slip 
angles of the front and rear tires, respectively. See 131] for 
more details. Referring to Figure [Tj the position Pco can be 
used as a differentially flat output 132]. Specifically, let m 
be the mass of the vehicle, the yaw moment of inertia, 
and I CO •= Izl'mlr. One can show that 


m Pco 


m R{'ip) 


Xx 

_ Vy + Vx'lp + IcO^ ^ 




where i?(-) is the 2D rotation matrix and u = 
is the fiat input comprising longitudinal force and 

latitudinal force Note that the fiat dynamics are for¬ 
mally identical to those of the unicycle model 0-0. By 
leveraging differential flatness, the idea is then to smooth a 
trajectory in the fiat output space and then map the fiat input 
u to the input ( = [S^Sfx, Srx]- Constraints for the fiat input 
u take the same form as in equations (|^-Q - the details 
can be found in 131]. When mapping u into [S^ Srx], 
under a no-drift assumption, only two real inputs can be 
uniquely determined, while the third is effectively a “degree 
of freedom,” see 131, Section II]. In this paper, we consider 
as degree of freedom the real input Srx- Us value is set 
equal to the solution of an optimization problem aimed at 
minimizing the tracking error with respect to the trajectory 
obtained with the fiat input u (the tracking error is due to 
the no-drift assumption). 

Figure 1^ shows the application of the CES algorithm to 
a 100m X 100m rocky terrain portrayed in Figure The 
parameters of the bicycle model are m = 1,725 kg, R = 
1, 300 kg • nff. If = 1.35 m, Ir = 1.15 m, and h = 0.3 m. 
In this case, the reference trajectory is computed by running 


































































Fig. 7: Definition of variables for bicycle model (adapted 
from [31]). 


differential FMT* with 1,000 samples on a unicycle model, 
as defined in equations O-©- The reference trajectory 
is discretized into 257 waypoints. The CES algorithm is 
then applied by using the aforementioned bicycle model 
and differential flatness transformation. Interestingly, in this 
example the smoothing algorithm is applied to a reference 
trajectory generated with a different (simpler) model - 
this, again, shows the robustness of the proposed approach. 
The length reduction was 4.19%, while the traversal time 
reduction was 39.74%. Computation times are reported in 
Table |T| Considering two iterations, the total smoothing 
time is 798 ms. Specifically, about 30% of the time is 
taken by differential FMT*, 20% by the shape optimization 
algorithm, and the remaining time by the bubble generation, 
the speed optimization, and the mapping via differential 
fiatness to a bicycle model. Remarkably, this result appears 
compatible with the real-time requirements of autonomous 
driving. Indeed, we note that the example in Figure is 
rather extreme in that a very long trajectory is planned amid 
several obstacles. In practical scenarios (e.g., urban driving), 
the planning problem may be simpler, implying that the 
computation times would be even lower. 


D. Elastic Stretching Moose Test 


In order to study in isolation the behavior of the Elastic 
Stretching algorithm, which is one of the main contributions 
of this paper, we compare our result with a trajectory 
consisting of clothoid splines. We note that the objective in 
the shape optimization step is not simply minimum length 
(which, for a unicycle model, produces paths consisting 
only of segments with maximum or zero curvature [33]), 
but instead encodes a notion of minimum overall curvature 
more compatible with dynamic considerations and speed 
optimization. To simplify the problem for finding a optimal 
solution with clothoid splines, we assume a constant vehicle 
speed along the trajectory. Fig. 10a illustrates a scenario 
of a simple Moose test (S shape turn). Here we consider 
a turning radius lower bound of 5m, and an upper b ound 
on the path curvature rate of change of Fig. 


10b 


shows the piecewise linear curvature profile for the clothoid 
trajectory. The results of the Elastic Stretching approach 
and the clothoid trajectory are very close in this illustrative 
example, with an error of 0.17% on the total path length. 



Time [ms] 

Global Planner 

357 

Bubble Generation 

93 (2 iterations) 

Shape Optimization 

203 (2 iterations) 

Speed Optimization 

31 (2 iterations) 

Mapping to Half-Car Model 

114 

Total 

798 


TABLE I: Computation times for planning and smoothing 
with a bicycle model. 


E. Discussion 

Overall, the above numerical experiments show three 
major trends. First, the smoothed trajectory often results 
in a noticeable length and traversal time reduction and, in 
general, a sequence of waypoints that may be easier to 
track for a lower-level controller. Second, the CES algorithm 
appears robust with respect to the model used to generate the 
initial reference trajectory. This is a fundamental property, 
as in practice one would use a motion planner on a simpler 
model (e.g., unicycle), and then run a smoothing algorithm 
with a more sophisticated model. Third, computation times 
are consistently below one second and in general appear 
compatible with a real-time implementation. 



Fig. 8: Smoothed trajectory in a rocky terrain with bicycle 
model (obstacles in work space) 



Fig. 9: Smoothed trajectory in a rocky terrain with bicycle 
model (obstacles infiated in configuration space) 

V. Conclusions 

In this paper we presented a novel algorithm. Convex 
Elastic Smoothing, for trajectory smoothing which 
alternates between shape and speed optimization. We showed 


























































































Clothoid curvature 


CES Pathlength : 5.98e+00 m 
Clothoid Pathlength : 5.99e+00 m 




(b) Curvature profile of the 
(a) Moose test simulation clothoid trajectory 

Fig. 10: Comparison of CES output and a clothoid trajectory. 


that both optimization problems can be solved via con¬ 
vex programming, which makes CES particularly fast and 
amenable to a real-time implementation. 

This paper leaves numerous important extensions open 
for further research. First, it is of interest to extend the 
CES algorithm to other dynamic systems, such as aerial 
vehicles or spacecraft. Second, we plan to investigate more 
thoroughly (1) the robustness of the algorithm when the ref¬ 
erence trajectory is not collision-free or dynamically-feasible 
and (2) the “typical” factor of suboptimality for a number 
of representative scenarios. Third, for shape optimization, 
this paper considered smoothness as the objective function. 
It is of interest to consider alternative objectives, which, 
for example, could reproduce the trajectories performed by 
race car drivers (such trajectories may involve significant 
curvature variations). Fourth, in an effort to make the pro¬ 
posed algorithm “trustworthy,” we plan to characterize upper 
bounds for computation times under suitable assumptions 
on the obstacle space. Finally, we plan to deploy the CES 
algorithm on real self-driving cars. 
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